/*!
 *****************************************************************
 * \file
 *
 * \note
 * Copyright (c) 2015 \n
 * Fraunhofer Institute for Manufacturing Engineering
 * and Automation (IPA) \n\n
 *
 *****************************************************************
 *
 * \note
 * Project name: Care-O-bot
 * \note
 * ROS stack name: autopnp
 * \note
 * ROS package name: ipa_room_segmentation
 *
 * \author
 * Author: Florian Jordan
 * \author
 * Supervised by: Richard Bormann
 *
 * \date Date of creation: 08.2015
 *
 * \brief
 *
 *
 *****************************************************************
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * - Redistributions of source code must retain the above copyright
 * notice, this list of conditions and the following disclaimer. \n
 * - Redistributions in binary form must reproduce the above copyright
 * notice, this list of conditions and the following disclaimer in the
 * documentation and/or other materials provided with the distribution. \n
 * - Neither the name of the Fraunhofer Institute for Manufacturing
 * Engineering and Automation (IPA) nor the names of its
 * contributors may be used to endorse or promote products derived from
 * this software without specific prior written permission. \n
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License LGPL as
 * published by the Free Software Foundation, either version 3 of the
 * License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU Lesser General Public License LGPL for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License LGPL along with this program.
 * If not, see <http://www.gnu.org/licenses/>.
 *
 ****************************************************************/

#include "rclcpp/rclcpp.hpp"
#include <string>
#include <vector>
#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>
#include <ipa_building_msgs/action/map_segmentation.hpp>
#include "rclcpp_action/rclcpp_action.hpp"

//#include <ipa_room_segmentation/RoomSegmentationConfig.h>
using ClientT=ipa_building_msgs::action::MapSegmentation;
enum class ActionStatus
{
    UNKNOWN = 0,
    PROCESSING = 1,
    FAILED = 2,
    SUCCEEDED = 3
};
ActionStatus current_goal_status_ = ActionStatus::UNKNOWN;
std::shared_ptr<ipa_building_msgs::action::MapSegmentation::Result> action_result = nullptr;

void resultCallback(const rclcpp_action::ClientGoalHandle<ClientT>::WrappedResult &result)
{

    switch (result.code)
    {
        case rclcpp_action::ResultCode::SUCCEEDED:
            current_goal_status_ = ActionStatus::SUCCEEDED;
            std::cout << "resultCallback current_goal_status_= " << (int) current_goal_status_ << std::endl;
            action_result = result.result;
            break;
        case rclcpp_action::ResultCode::ABORTED:
            current_goal_status_ = ActionStatus::FAILED;
            std::cout << "resultCallback current_goal_status_= " << (int) current_goal_status_ << std::endl;
            break;
        case rclcpp_action::ResultCode::CANCELED:
            current_goal_status_ = ActionStatus::FAILED;
            std::cout << "resultCallback current_goal_status_= " << (int) current_goal_status_ << std::endl;
            break;
        default:
            current_goal_status_ = ActionStatus::UNKNOWN;
            std::cout << "resultCallback current_goal_status_= " << (int) current_goal_status_ << std::endl;
            break;
    }
}

void goalResponseCallback(const rclcpp_action::ClientGoalHandle<ClientT>::SharedPtr &future)
{
    auto goal_handle = future.get();
    if (!goal_handle)
    {
        current_goal_status_ = ActionStatus::FAILED;
        std::cout << "action client failed to send goal to server." << std::endl;
        return;
    }
    std::cout << "action client success to send goal to server." << std::endl;
}


int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto nh = rclcpp::Node::make_shared("room_segmentation_client");
    rclcpp_action::Client<ClientT>::SharedPtr room_segmentation_client;
    room_segmentation_client = rclcpp_action::create_client<ClientT>(nh, "room_segmentation_server");
    auto send_goal_options = rclcpp_action::Client<ClientT>::SendGoalOptions();
    send_goal_options.result_callback = resultCallback;
    send_goal_options.goal_response_callback = goalResponseCallback;
    // map names
    std::vector<std::string> map_names;
    map_names.push_back("lab_ipa");
    map_names.push_back("lab_c_scan");
    map_names.push_back("Freiburg52_scan");
    map_names.push_back("Freiburg79_scan");
    map_names.push_back("lab_b_scan");
    map_names.push_back("lab_intel");
    map_names.push_back("Freiburg101_scan");
    map_names.push_back("lab_d_scan");
    map_names.push_back("lab_f_scan");
    map_names.push_back("lab_a_scan");
    map_names.push_back("NLB");
    map_names.push_back("office_a");
    map_names.push_back("office_b");
    map_names.push_back("office_c");
    map_names.push_back("office_d");
    map_names.push_back("office_e");
    map_names.push_back("office_f");
    map_names.push_back("office_g");
    map_names.push_back("office_h");
    map_names.push_back("office_i");
    map_names.push_back("lab_ipa_furnitures");
    map_names.push_back("lab_c_scan_furnitures");
    map_names.push_back("Freiburg52_scan_furnitures");
    map_names.push_back("Freiburg79_scan_furnitures");
    map_names.push_back("lab_b_scan_furnitures");
    map_names.push_back("lab_intel_furnitures");
    map_names.push_back("Freiburg101_scan_furnitures");
    map_names.push_back("lab_d_scan_furnitures");
    map_names.push_back("lab_f_scan_furnitures");
    map_names.push_back("lab_a_scan_furnitures");
    map_names.push_back("NLB_furnitures");
    map_names.push_back("office_a_furnitures");
    map_names.push_back("office_b_furnitures");
    map_names.push_back("office_c_furnitures");
    map_names.push_back("office_d_furnitures");
    map_names.push_back("office_e_furnitures");
    map_names.push_back("office_f_furnitures");
    map_names.push_back("office_g_furnitures");
    map_names.push_back("office_h_furnitures");
    map_names.push_back("office_i_furnitures");
    std::string image_filename;
    for (size_t image_index = 0; image_index < 1; ++image_index)
    {
//    for (size_t image_index = 0; image_index < map_names.size(); ++image_index) {
        // import maps
        //image_filename = "/home/ora/ros2_ws/src/Sweeping_robot/ipa_coverage_planning/ipa_room_segmentation/common/files/test_maps/" + map_names[image_index] + ".png";
        image_filename = "/home/ubuntu/ros2_ws/src/Sweeping_robot/ipa_coverage_planning/ipa_room_segmentation/common/files/test_maps/map2.pgm";
        std::cout << "current pic is : " << image_filename << std::endl;
        cv::Mat map = cv::imread(image_filename.c_str(), 0);
        //make non-white pixels black
        for (int y = 0; y < map.rows; y++)
        {
            for (int x = 0; x < map.cols; x++)
            {
                //find not reachable regions and make them black
                if (map.at<unsigned char>(y, x) < 250)
                {
                    map.at<unsigned char>(y, x) = 0;
                }
                    //else make it white
                else
                {
                    map.at<unsigned char>(y, x) = 255;
                }
            }
        }
//		cv::imshow("map", map);
//		cv::waitKey();
        sensor_msgs::msg::Image labeling;
        cv_bridge::CvImage cv_image;
        //	cv_image.header.stamp = ros::Time::now();
        cv_image.encoding = "mono8";
        cv_image.image = map;
        cv_image.toImageMsg(labeling);

        // create the action client --> "name of server"
        // true causes the client to spin its own thread
        printf("Waiting for action server to start.\n");
        // wait for the action server to start


        // test dynamic reconfigure
        //DynamicReconfigureClient drc(nh, "room_segmentation_server/set_parameters", "room_segmentation_server/parameter_updates");
        //drc.setConfig("room_segmentation_algorithm", 3);
//		drc.setConfig("display_segmented_map", true);
        //drc.setConfig("room_area_factor_upper_limit_voronoi", 120.0);

        // send a goal to the action

        while (rclcpp::ok() && !room_segmentation_client->wait_for_action_server(std::chrono::seconds(5)))
        {
            std::cout << "Action server not available after waiting, wait 5s..." << std::endl;
        }
        printf("Action server started, sending goal.\n");
        ipa_building_msgs::action::MapSegmentation::Goal goal;
        goal.data_source = goal.IMAGE_DATA;
        goal.input_map = labeling;
        goal.map_origin.position.x = -5.75;
        goal.map_origin.position.y = -5.71;
        goal.map_resolution = 0.05;
        goal.return_format_in_meter = false;
        goal.return_format_in_pixel = true;
        goal.robot_radius = 0.15;

        room_segmentation_client->async_send_goal(goal, send_goal_options);
        current_goal_status_ = ActionStatus::PROCESSING;
        rclcpp::Rate r(5);
        while (rclcpp::ok())
        {
            std::cout << "current_goal_status_= " << (int) current_goal_status_ << std::endl;
            if (current_goal_status_ == ActionStatus::SUCCEEDED)
            {
                printf("Finished successfully!");
                // display
                cv_bridge::CvImagePtr cv_ptr_obj;
                cv_ptr_obj = cv_bridge::toCvCopy(action_result->segmented_map,
                                                 sensor_msgs::image_encodings::TYPE_32SC1);
                cv::Mat segmented_map = cv_ptr_obj->image;
                cv::Mat colour_segmented_map = segmented_map.clone();
                colour_segmented_map.convertTo(colour_segmented_map, CV_8U);

                /* for(size_t i = 1; i <= action_result->room_information_in_pixel.size(); ++i)
                {
                    cv::Mat map_copy(colour_segmented_map == i);

                    for (int y = 0; y < map_copy.rows; y++)
                    {
                        for (int x = 0; x < map_copy.cols; x++)
                        {
                            if (map_copy.at<unsigned char>(y, x) > 0)
                            {
                                map_copy.at<unsigned char>(y, x) = 255;
                            }
                            else
                            {
                                map_copy.at<unsigned char>(y, x) = 0;
                            }
                        }
                    }
                    cv::namedWindow("aaa",0);
                    cv::resizeWindow("aaa", 1000, 1000);
                    cv::imshow("aaa", map_copy);
                    std::cout<<"ori cols= " <<colour_segmented_map.cols  << " rows= " <<colour_segmented_map.rows<<std::endl;
                    std::cout<<"map_copy cols= " <<map_copy.cols  << " rows= " <<map_copy.rows<<std::endl;
                    cv::waitKey(0) ;
                } */

                cv::cvtColor(colour_segmented_map, colour_segmented_map, CV_GRAY2BGR);


                for (size_t i = 1; i <= action_result->room_information_in_pixel.size(); ++i)
                {
                    //choose random color for each room
                    int blue = (rand() % 250) + 1;
                    int green = (rand() % 250) + 1;
                    int red = (rand() % 250) + 1;
                    for (size_t u = 0; u < segmented_map.rows; ++u)
                    {
                        for (size_t v = 0; v < segmented_map.cols; ++v)
                        {
                            if (segmented_map.at<int>(u, v) == i)
                            {
                                colour_segmented_map.at<cv::Vec3b>(u, v)[0] = blue;
                                colour_segmented_map.at<cv::Vec3b>(u, v)[1] = green;
                                colour_segmented_map.at<cv::Vec3b>(u, v)[2] = red;
                            }
                        }
                    }
                }
                //draw the room centers into the map
                for (size_t i = 0; i < action_result->room_information_in_pixel.size(); ++i)
                {
                    cv::Point current_center(action_result->room_information_in_pixel[i].room_center.x,
                                             action_result->room_information_in_pixel[i].room_center.y);
                    cv::circle(colour_segmented_map, current_center, 2, CV_RGB(0, 0, 255), cv::FILLED);
                }

                cv::imshow("segmentation client: ", colour_segmented_map);
                cv::waitKey();
                break;
            } else if (current_goal_status_ == ActionStatus::FAILED)
            {
                std::cout << "break :  current_goal_status_ == ActionStatus::FAILED" << std::endl;
                break;
            }
            r.sleep();
            rclcpp::spin_some(nh);
        }
    }
    //exit
    return 0;
}
